#include "StateMachine.h"
#include "ros/ros.h"
#include <std_msgs/Int32.h>
#include <unistd.h>
#include <math.h>
//#include <stdio.h>

ros::Publisher pub_state;

int state = START;

bool is_in_signal(Signal s, int inputSignal)
{
  return (((1<<s)&inputSignal) > 0);
}

void state_transitions(const std_msgs::Int32::ConstPtr& signal)
{
  int oldState = state;
  switch(state)
  {
    case START:
      if (is_in_signal(ACTIVATE1, signal->data))
      {
	      state = FIND_FIRST_WALL;
      }
      if (is_in_signal(ACTIVATE2, signal->data) || is_in_signal(FOLLOW_PATH_STRAIGHT, signal->data))
      {
        state = PATH_STRAIGHT;
      }
      break;
    case FIND_FIRST_WALL:
      if (is_in_signal(BUMPER, signal->data))
      {
        state = WOWOWO;
      }
      else if (is_in_signal(C_C_FRONT, signal->data) || is_in_signal(F_C_FRONT, signal->data))
      {
        state = CORNER;
      }
      else if (is_in_signal(C_C_RIGHT, signal->data))
      {
        state = STRAIGHT_WALL;
      }
      else if (is_in_signal(C_F_RIGHT, signal->data))
      {
        state = PART_WALL_FRONT;
      } 
      else if (is_in_signal(F_C_RIGHT, signal->data))
      {
        state = PART_HOLE_FRONT;
      }
      break;  
    case STRAIGHT_WALL:
      if (is_in_signal(BUMPER, signal->data))
      {
        state = WOWOWO;
      }
      else if (is_in_signal(TAG_DETECTED, signal->data))
      {
	      state = TAG_HANDLE;
      }	
      else if (is_in_signal(C_C_FRONT, signal->data) || is_in_signal(F_C_FRONT, signal->data))
      {
        state = CORNER;
      }
      else if (is_in_signal(F_C_RIGHT, signal->data))
      {
        state = PART_HOLE_FRONT;
      }
          
      break;
      
    case CORNER:
    
      if (is_in_signal(MOTOR_DONE, signal->data))
      {
        state = STRAIGHT_WALL;
      }
      
      break;
      
    case PART_HOLE_FRONT:
      if (is_in_signal(BUMPER, signal->data))
      {
        state = WOWOWO;
      }
      else if (is_in_signal(C_C_FRONT, signal->data) || is_in_signal(F_C_FRONT, signal->data))
      {
        state = CORNER;
      }
      else if (is_in_signal(F_F_RIGHT, signal->data))
      {
        state = HOLE;
      }
      break;
      
    case HOLE:
    
      if (is_in_signal(MOTOR_DONE, signal->data))
      {
        state = FIND_WALL;
      }
      
      break;
      
    case FIND_WALL: 
      if (is_in_signal(BUMPER, signal->data))
      {
        state = WOWOWO;
      }
      else if (is_in_signal(C_C_FRONT, signal->data) || is_in_signal(F_C_FRONT, signal->data))
      {
        state = CORNER;
      }
      else if (is_in_signal(C_F_RIGHT, signal->data))
      {
        state = PART_WALL_FRONT;
      }
      break;
      
    case PART_WALL_FRONT:
      if (is_in_signal(BUMPER, signal->data))
      {
        state = WOWOWO;
      }
      else if (is_in_signal(C_C_FRONT, signal->data) || is_in_signal(F_C_FRONT, signal->data))
      {
        state = CORNER;
      }
      else if (is_in_signal(C_C_RIGHT, signal->data))
      {
        state = STRAIGHT_WALL;
      }
      else if (is_in_signal(F_F_RIGHT, signal->data))
      {
        state = PART_WALL_PART_HOLE;
      }
      break;
    case PART_WALL_PART_HOLE:
      if (is_in_signal(BUMPER, signal->data))
      {
        state = WOWOWO;
      }
      else if (is_in_signal(C_C_FRONT, signal->data) || is_in_signal(F_C_FRONT, signal->data))
      {
        state = CORNER;
      }
      else if (is_in_signal(F_C_RIGHT, signal->data))
      {
        state = PART_HOLE_FRONT;
      }
      break;
     case TAG_HANDLE:
       printf("Tag handle \n");
       if (is_in_signal(TAG_DETECTION_FINISHED, signal->data))
       {
        //printf("finished Tag handling \n");
        //sleep(10);
        state = TAG_PROCESSING;
       }
       break;
      case TAG_PROCESSING:
        if (is_in_signal(TAG_PROCESSING_FINISHED, signal->data))
        {
          sleep(1);
          state = STRAIGHT_WALL;
        }
      break;
      case PATH_STRAIGHT:
        if (is_in_signal(BUMPER, signal->data))
        {
          state = WOWOWO_PATH;
        }
        else if (is_in_signal(FOLLOW_PATH_LEFT, signal->data))
        {
          state = PATH_LEFT_TURN;
        }
        else if (is_in_signal(FOLLOW_PATH_RIGHT, signal->data))
        {
          state = PATH_RIGHT_TURN;
        }
        else if (is_in_signal(FOLLOW_PATH_U_TURN, signal->data))
        {
          state = PATH_U_TURN;
        }
        else if (is_in_signal(FOLLOW_PATH_SEARCH_TAG, signal->data))
        {
          state = PATH_SEARCH_TAG;
        }
      break;
      case PATH_LEFT_TURN:
        if (is_in_signal(MOTOR_DONE, signal->data))
        {
          state = PATH_STRAIGHT;
        }
      break;
      case PATH_RIGHT_TURN:
        if (is_in_signal(MOTOR_DONE, signal->data))
        {
          state = PATH_STRAIGHT;
        }      
      break;
      case PATH_U_TURN:
        if (is_in_signal(MOTOR_DONE, signal->data))
        {
          state = PATH_STRAIGHT;
        }  
      break;
      case PATH_SEARCH_TAG:
      break;
      case WOWOWO:
        if (is_in_signal(MOTOR_DONE, signal->data))
        {
          state = WOWOWO_TURN;
        }
      break;
      case WOWOWO_TURN:
        if (is_in_signal(MOTOR_DONE, signal->data))
        {
          state = FIND_FIRST_WALL;
        }
      break;
      case WOWOWO_PATH:
        if (is_in_signal(FOLLOW_PATH_LEFT, signal->data))
        {
          state = PATH_LEFT_TURN;
        }
        else if (is_in_signal(FOLLOW_PATH_RIGHT, signal->data))
        {
          state = PATH_RIGHT_TURN;
        }
        else if (is_in_signal(FOLLOW_PATH_U_TURN, signal->data))
        {
          state = PATH_U_TURN;
        }
        else if (is_in_signal(FOLLOW_PATH_SEARCH_TAG, signal->data))
        {
          state = PATH_SEARCH_TAG;
        }
      break;
    default:
      break;
  }
  
  if (oldState != state)
  {
    std_msgs::Int32 pub;
    pub.data = state;
    printf("New state: %d\n", pub.data);
    pub_state.publish(pub); 
    
  }
  
  // :) We are happy about this stuff
  
}

int main(int argc, char**argv) {
  ros::init(argc, argv, "StateMachine");
  ros::NodeHandle n;

  // When state signal changes check if it's time for a state transition
  ros::Subscriber sub = n.subscribe("Signals/state_signal", 100, state_transitions);

  pub_state = n.advertise<std_msgs::Int32>("State/Info", 1);
  printf("StateMachine running\n");
  ros::spin();
}
